1-D to n-D

1-Dimensional Graphs

In the previous examples, you were working with 1-dimensional graphs. The robot's motion and measurements were limited to one dimension - they could either be performed forwards or backwards.

In such a case, each constraint could be represented in the following form,

1-D Measurement constraint: \large \frac{(z_t - (x_t + m_t))^2}{\sigma^2}

1-D Motion constraint: \large \frac{(x_t - (x_{t-1} + u_t))^2}{\sigma^2}

n-Dimensional Graphs

In multi-dimensional systems, we must use matrices and covariances to represent the constraints. This generalization can be applied to system of 2-dimensions, 3-dimensions, and really any n-number of dimensions. The equations for the constraints would look like so,

**n-D Measurement constraint: **(z_t-h(x_t,m_j))^TQ_t^{-1}(z_t-h(x_t,m_j))

n-D Motion constraint: (x_t-g(u_t,x_{t-1}))^TR_t^{-1}(x_t-g(u_t,x_{t-1}))

Where h() and g() represent the measurement and motion functions, and Q_t and R_t are the covariances of the measurement and motion noise. These naming conventions should be familiar to you, as they were all introduced in the Localization module.

The multidimensional formula for the sum of all constraints is presented below.

J_{GraphSLAM} = x_0^T \Omega x_0 + \sum_t(x_t-g(u_t,x_{t-1}))^TR_t^{-1}(x_t-g(u_t,x_{t-1}))
\quad + \sum_t(z_t-h(x_t,m_j))^TQ_t^{-1}(z_t-h(x_t,m_j))

The first element in the sum is the initial constraint - it sets the first robot pose to equal to the origin of the map. The covariance, \Omega_0, represents complete confidence. Essentially,

Now that we are working with multi-dimensional graphs and multi-dimensional constraints, it makes sense to use a more intelligent data structure to work with our data. The information matrix and information vector are just that!